// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URControlCommandBuilder.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.UniversalRobots
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Builds command strings for issuing commands to URControl server.
    /// </summary>
    public static class URControlCommandBuilder
    {
        /// <summary>
        /// Construct the command string for a Cartesian position command.
        /// </summary>
        /// <param name="cartesianPositionGoal">Cartesian goal as [X Y Z Rx Ry Rz]</param>
        /// <returns>Command string.</returns>
        public static string ConstructCartesianPositionGoalString(double[] cartesianPositionGoal)
        {
            StringBuilder sb = new StringBuilder();

            sb.Append("movej(p[");
            for (int i = 0; i < 5; ++i)
            {
                sb.Append(cartesianPositionGoal[i].ToString());
                sb.Append(", ");
            }

            sb.Append(cartesianPositionGoal[5].ToString());
            sb.Append("])\n");

            TraceOut.Verbose(TraceContexts.HW, "UR Cartesian position command: " + sb.ToString());
            return sb.ToString();
        }

        /// <summary>
        /// Construct the command string for a joint position command.
        /// </summary>
        /// <param name="jointPositionGoals">Target joint positions.</param>
        /// <param name="a"> Acceleration parameter. </param>
        /// <param name="v"> Velocity of leading joint. </param>
        /// <param name="r"> Blend radius. </param>
        /// <returns>String for joint position command.</returns>
        public static string ConstructJointPositionGoalString(double[] jointPositionGoals, double a = 1.2, double v = 0.3, double r = 0)
        {
            StringBuilder sb = new StringBuilder();

            sb.Append("movej([");
            for (int i = 0; i < URControlClient.NumJoints - 1; ++i)
            {
                sb.Append(jointPositionGoals[i].ToString());
                sb.Append(", ");
            }

            sb.Append(jointPositionGoals[URControlClient.NumJoints - 1].ToString());

            // extra parameters
            sb.Append("],");
            sb.Append(a.ToString());
            sb.Append(",");
            sb.Append(v.ToString());
            sb.Append(",");
            sb.Append(r.ToString());
            sb.Append(")\n");

            TraceOut.Verbose(TraceContexts.HW, "UR joint position command: " + sb.ToString());
            return sb.ToString();
        }

        /// <summary>
        /// Construct the command string for a joint velocity command.
        /// </summary>
        /// <param name="jointVelocities">Reference joint velocities in rad/s.</param>
        /// <param name="acceleration">Acceleration of leading axis in rad/s^2</param>
        /// <param name="timeout">Timeout in seconds.</param>
        /// <returns>String for joint velocity command.</returns>
        public static string ConstructJointVelocityString(double[] jointVelocities, double acceleration, double timeout)
        {
            StringBuilder sb = new StringBuilder();

            sb.Append("speedj([");
            for (int i = 0; i < URControlClient.NumJoints - 1; ++i)
            {
                sb.Append(jointVelocities[i].ToString());
                sb.Append(", ");
            }

            sb.Append(jointVelocities[URControlClient.NumJoints - 1].ToString());
            sb.Append("],");

            sb.Append(acceleration.ToString());

            sb.Append(",");

            sb.Append(timeout.ToString());
            sb.Append(")\n");

            TraceOut.Verbose(TraceContexts.HW, "UR joint speed command: " + sb.ToString());
            return sb.ToString();
        }
    }
}
